#include "math_func.h"

static  void UInt8ToInt32(const uint8_t* src, int32_t* dest,int32_t len) 
{
	int32_t i;
    for ( i = 0; i < len; i++)
      *(dest++) = (*(src++));
 }
static  void VectorAdd(const int32_t* x, const int32_t* y, int32_t* z,int32_t len) 
{
	  __m128i x1;
	  __m128i y1;
	  const __m128i* x2 = (const __m128i*)(x);
	  const __m128i* y2 = (const __m128i*)(y);
	  __m128i* z2 = (__m128i*)(z);

	  int32_t i;
	  for (i = 0; i < len - 4; i += 4) {
		  x1 = _mm_loadu_si128(x2++);
		  y1 = _mm_loadu_si128(y2++);
		  _mm_storeu_si128(z2++, _mm_add_epi32(x1, y1));
	  }
	  for (; i < len; i++)
		  *(z + i) = (*(x + i)) + (*(y + i));
}

static  void VectorSub(const int32_t* x, const int32_t* y, int32_t* z, int32_t len) 
{
	  __m128i x1;
	  __m128i y1;
	  const __m128i* x2 = (const __m128i*)(x);
	  const __m128i* y2 = (const __m128i*)(y);
	  __m128i* z2 = (__m128i*)(z);

	  int32_t i;
	  for (i = 0; i < len - 4; i += 4) {
		  x1 = _mm_loadu_si128(x2++);
		  y1 = _mm_loadu_si128(y2++);

		  _mm_storeu_si128(z2++, _mm_sub_epi32(x1, y1));
	  }
	  for (; i < len; i++)
	  {
		  *(z + i) = (*(x + i)) - (*(y + i));
		  //printf("%d, ", *(z + i));
	  }
		  
			
  }

 static  void VectorAbs(const int32_t* src, int32_t* dest, int32_t len) 
 {
	  __m128i val;
	  __m128i val_abs;
	  const __m128i* x = (const __m128i*)(src);
	  __m128i* y = (__m128i*)(dest);

	  int32_t i;
	  for (i = 0; i < len - 4; i += 4) {
		  val = _mm_loadu_si128(x++);
		  val_abs = _mm_abs_epi32(val);
		  _mm_storeu_si128(y++, val_abs);
	  }
	  for (; i < len; i++)
		  dest[i] = (src[i] >= 0 ? src[i] : -src[i]);
  }

  static  void Square(const int32_t* src, uint32_t* dest, int32_t len) {
	  __m128i x1;
	  const __m128i* x2 = (const __m128i*)(src);
	  __m128i* y2 = (__m128i*)(dest);

	  int32_t i;
	  for (i = 0; i < len - 4; i += 4) {
		  x1 = _mm_loadu_si128(x2++);
		  _mm_storeu_si128(y2++, _mm_mullo_epi32(x1, x1));
	  }
	  for (; i < len; i++)
		  *(dest + i) = (*(src + i)) * (*(src + i));
  }

static  float VectorInnerProduct(const float* x, const float* y,int32_t len) 
{
	  __m128 x1;
	  __m128 y1;
	  __m128 z1 = _mm_setzero_ps();
	  float prod;
	  float buf[4];

	  int32_t i;
	  for (i = 0; i < len - 4; i += 4) {
		  x1 = _mm_loadu_ps(x + i);
		  y1 = _mm_loadu_ps(y + i);
		  z1 = _mm_add_ps(z1, _mm_mul_ps(x1, y1));
	  }
	  _mm_storeu_ps(&buf[0], z1);
	  prod = buf[0] + buf[1] + buf[2] + buf[3];
	  for (; i < len; i++)
		  prod += x[i] * y[i];

	  return prod;
}

